#include "tpa626.hpp"

yj_return tpa626::_read_reg(reg_addr reg, uint16_t *data, uint8_t len)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_reg:i2c is not ready";
        return YJ_ERROR;
    }

    if (!data)
    {
        LOG(ERROR) << "❌ _read_reg:data is null";
        return YJ_ERROR;
    }
    uint8_t *u8_array = reinterpret_cast<uint8_t *>(data);
    uint32_t u8_len = len * sizeof(uint16_t); // 字节长度 = 元素数 × 2

    if (_i2c->read_register(static_cast<uint8_t>(reg), u8_array, u8_len) != YJ_OK)
    {
        LOG(ERROR) << "❌ _read_reg:tpa626 read failed";
        return YJ_ERROR;
    }

    // 交换字节顺序
    uint16_t temp;
    for (int i = 0; i < len; i++)
    {
        temp = data[i];
        data[i] = (temp >> 8) | (temp << 8);
    }

    return YJ_OK;
}

yj_return tpa626::_write_reg(reg_addr reg, uint16_t *data, uint8_t len)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_reg:i2c is not ready";
        return YJ_ERROR;
    }

    if (!data)
    {
        LOG(ERROR) << "❌ _read_reg:data is null";
        return YJ_ERROR;
    }
    // 交换字节顺序
    uint16_t temp;
    uint16_t u16_temp[len];
    for (int i = 0; i < len; i++)
    {
        temp = data[i];
        u16_temp[i] = (temp >> 8) | (temp << 8);
    }

    uint8_t *u8_array = reinterpret_cast<uint8_t *>(u16_temp);
    uint32_t u8_len = len * sizeof(uint16_t); // 字节长度 = 元素数 × 2

    if (_i2c->write_register(static_cast<uint8_t>(reg), u8_array, u8_len) != YJ_OK)
    {
        LOG(ERROR) << "❌ _write_reg:tpa626 write failed";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return tpa626::read_single_reg(reg_addr reg, uint16_t *reg_out)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ _read_single_reg:device i2c is not open";
        return YJ_ERROR;
    }
    if (!reg_out)
    {
        LOG(ERROR) << "❌ _read_single_reg:tpa626 reg_out is null";
        return YJ_ERROR;
    }

    if (_read_reg(reg, reg_out, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ _read_single_reg:tpa626 read reg failed";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return tpa626::check_MIF_id_and_DIE_id()
{
    if (_flag != YJ_OK)
    {
        LOG(WARNING) << "⚠️ check_MIF_id_and_DIE_id:tpa626 flag is not ok";
    }

    uint16_t mif, die;

    if (_read_reg(mf_id_reg_addr, &mif, 1) != YJ_OK)
        return YJ_ERROR;
    if (_read_reg(die_id_reg_addr, &die, 1) != YJ_OK)
        return YJ_ERROR;
    if (mif != 0x5549 || die != 0x2260)
    {
        LOG(ERROR) << "❌ check_MIF_id_and_DIE_id:tpa626 mif or die id is not correct";
        LOG(ERROR) << "MIF: " << std::hex << mif << " DIE: " << std::hex << die;
        return YJ_ERROR;
    }

    return YJ_OK;
}

tpa626::tpa626(std::string dev_name,
               i2c_v1 *i2c,
               uint8_t device_address,
               double max_measure_current,
               double shunt_res) : _dev_name(dev_name),
                                   _i2c(i2c),
                                   _device_address(device_address),
                                   _max_measure_current(max_measure_current),
                                   _shunt_res(shunt_res)
{
    _flag = YJ_ERROR;
}
tpa626::~tpa626()
{
    if (_i2c)
    {
        _i2c->close();
    }
}

yj_return tpa626::read_status()
{
    return _flag;
}
yj_return tpa626::init()
{
    double current_lsb, cal;
    uint16_t reg_buff;

    if (_i2c->open() != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 i2c open failed";
        goto end;
    }

    if (_i2c->set_address(_device_address) != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 i2c set address failed";
        goto end;
    }

    if (check_MIF_id_and_DIE_id() != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 is not online";
        goto end;
    }

    if (reset() != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 reset failed";
        goto end;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // if (_read_reg(config_reg_addr, reinterpret_cast<uint16_t *>(&config_reg_temp), 1) != YJ_OK)
    // {
    //     LOG(ERROR) << "❌ init:tpa626 read config reg failed";
    //     goto end;
    // }
    reg_buff = 0;
    // 16次采样平均
    reg_buff |= (1 << 10);
    // config_reg_temp.bit11 = 0;
    // config_reg_temp.bit10 = 1;
    // config_reg_temp.bit9 = 0;

    // bus采样时间2.116ms
    reg_buff |= (1 << 8);
    reg_buff |= (1 << 6);
    // config_reg_temp.bit8 = 1;
    // config_reg_temp.bit7 = 0;
    // config_reg_temp.bit6 = 1;

    // 采样电阻采样时间2.116ms
    reg_buff |= (1 << 3);
    reg_buff |= (1 << 5);
    // config_reg_temp.bit5 = 1;
    // config_reg_temp.bit4 = 0;
    // config_reg_temp.bit3 = 1;

    // 采样总线电压和采样电阻电压
    // config_reg_temp.bit2 = 1;
    // config_reg_temp.bit1 = 1;
    // config_reg_temp.bit0 = 1;
    reg_buff |= (1 << 0);
    reg_buff |= (1 << 1);
    reg_buff |= (1 << 2);

    if (_write_reg(config_reg_addr, &reg_buff, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 write config reg failed";
        goto end;
    }

    // 写校准寄存器
    // 计算校准寄存器值
    current_lsb = _max_measure_current / pow(2, 15);
    cal = 0.00512 / current_lsb / _shunt_res;

    reg_buff = static_cast<uint16_t>(cal);

    if (_write_reg(calibration_reg_addr, (&reg_buff), 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ init:tpa626 write calibration reg failed";
        goto end;
    }
    _shunt_calibation = reg_buff;
    _current_lsb = 0.00512 / _shunt_res / reg_buff;

    LOG(INFO) << "✔️ tpa626 init ok";
    print_config();
    _flag = YJ_OK;
    return YJ_OK;
end:
    _flag = YJ_ERROR;
    return YJ_ERROR;
}

yj_return tpa626::read_config_reg(uint16_t *config_reg)
{
    return read_single_reg(config_reg_addr, config_reg);
}

yj_return tpa626::read_calibration_reg(uint16_t *calibration_reg)
{
    return read_single_reg(calibration_reg_addr, calibration_reg);
}

yj_return tpa626::check_calibration_reg_is_ok()
{
    uint16_t cal_temp;
    if (read_calibration_reg(&cal_temp) != YJ_OK)
    {
        LOG(ERROR) << "❌ check_calibration_reg_is_ok:tpa626 read calibration reg failed";
        return YJ_ERROR;
    }
    if (cal_temp != _shunt_calibation)
    {
        LOG(ERROR) << "❌ check_calibration_reg_is_ok:tpa626 calibration reg is not equal to init value";
        LOG(ERROR) << "cal_temp: 0x" << std::hex << std::setfill('0') << std::setw(4) << std::uppercase << cal_temp
                   << " _shunt_calibation: 0x" << std::hex << std::setfill('0') << std::setw(4) << std::uppercase << _shunt_calibation;
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return tpa626::reset()
{
    uint16_t config_reg;
    if (_flag == YJ_OK)
    {
        LOG(ERROR) << "❌ reset:tpa626 flag is not ok";
        return YJ_ERROR;
    }
    if (_read_reg(config_reg_addr, &config_reg, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:tpa626 read config reg error";
        return YJ_ERROR;
    }

    config_reg |= (1 << 15);

    if (_write_reg(config_reg_addr, &config_reg, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:tpa626 write config reg error";
        return YJ_ERROR;
    }

    _flag = YJ_ERROR;
    return YJ_OK;
}

yj_return tpa626::read_power(double *power)
{
    uint16_t reg_buff;
    if (_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power:tpa626 flag is not ok";
        return YJ_ERROR;
    }

    if (!power)
    {
        LOG(ERROR) << "❌ read_power:tpa626 power is null";
        return YJ_ERROR;
    }

    if (_read_reg(power_reg_addr, &reg_buff, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power:tpa626 read power reg error";
        return YJ_ERROR;
    }

    *power = reg_buff * _current_lsb * 25;
    return YJ_OK;
}
yj_return tpa626::read_current(double *current)
{
    uint16_t reg_buff;
    if (_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ read_current:tpa626 flag is not ok";
        return YJ_ERROR;
    }

    if (!current)
    {
        LOG(ERROR) << "❌ read_current:tpa626 current is null";
        return YJ_ERROR;
    }

    if (_read_reg(current_reg_addr, &reg_buff, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_current:tpa626 read current reg error";
        return YJ_ERROR;
    }

    *current = reg_buff * _current_lsb;
    return YJ_OK;
}
yj_return tpa626::read_bus_vol(double *bus_vol)
{
    uint16_t reg_buff;
    if (_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ read_bus_vol:tpa626 flag is not ok";
        return YJ_ERROR;
    }

    if (!bus_vol)
    {
        LOG(ERROR) << "❌ read_bus_vol:tpa626 bus_vol is null";
        return YJ_ERROR;
    }

    if (_read_reg(bus_vol_reg_addr, &reg_buff, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_bus_vol:tpa626 read bus vol reg error";
        return YJ_ERROR;
    }

    *bus_vol = reg_buff * 1.25 / 1000;
    return YJ_OK;
}
yj_return tpa626::read_power_current_bus_vol(double *power, double *current, double *bus_vol)
{
    uint16_t reg_buff1, reg_buff2, reg_buff3;
    if (_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power_current_bus_vol:tpa626 flag is not ok";
        return YJ_ERROR;
    }

    if (!power || !current || !bus_vol)
    {
        LOG(ERROR) << "❌ read_power_current_bus_vol:tpa626 power or current or bus_vol is null";
        return YJ_ERROR;
    }

    if (_read_reg(power_reg_addr, &reg_buff1, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power_current_bus_vol:tpa626 read power reg error";
        return YJ_ERROR;
    }

    if (_read_reg(current_reg_addr, &reg_buff2, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power_current_bus_vol:tpa626 read current reg error";
        return YJ_ERROR;
    }

    if (_read_reg(bus_vol_reg_addr, &reg_buff3, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_power_current_bus_vol:tpa626 read bus vol reg error";
        return YJ_ERROR;
    }
    *power = reg_buff1 * _current_lsb * 25;
    *current = reg_buff2 * _current_lsb;
    *bus_vol = reg_buff3 * 1.25 / 1000;
    return YJ_OK;
}

void tpa626::print_config()
{
    LOG(INFO) << "=======================tpa626 config=======================";
    LOG(INFO) << "dev name: " << _dev_name;
    LOG(INFO) << "i2c bus name: " << _i2c->get_dev_path();
    LOG(INFO) << "i2c addr: 0x" << std::hex << std::setfill('0') << std::setw(2) << std::uppercase << static_cast<uint16_t>(_device_address);
    LOG(INFO) << "max current: " << _max_measure_current << " A";
    LOG(INFO) << "current res: " << _shunt_res << " Ω";
    LOG(INFO) << "calibrated res value: 0x" << std::hex << std::setfill('0') << std::setw(4) << std::uppercase << _shunt_calibation;
    LOG(INFO) << "current_lsb: " << _current_lsb;
    LOG(INFO) << "=======================tpa626 config=======================";
}